package novemberBot;

import novemberBot.utils.Timer2;
import novemberBot.utils.Trig;
import lejos.nxt.Motor;
import lejos.nxt.TachoMotorPort;
import lejos.util.TimerListener;

/**allows for more precise control of specific motors 
 * not intended for wheels but rather motors with limited
 * range of motion, a tendency to get stuck or containing gear
 * ratios. Also if autoStop is on the motor regulator is not used.
 * unfortunately because of the way the regulator works it is incompatible
 * with the autoStop feature
 * @author Antoine Lennartz*/
public class EnhancedMotor extends Motor {
	/**holds the gear ratio from the motor to the final gear*/
	private double gearRatio;
	
	/**serves to check if the motor is stuck*/
	private MotorChecker mc;
	
	private NoRegulator nr = new NoRegulator(this);
	
	/**Determines weather or not the robot autoStops after failing to complete the motion
	 * in reasonable time. Defaults to true*/
	private boolean autoStop=false;
	
	/**Constructor for EnhancedMotor*/
	public EnhancedMotor(TachoMotorPort port, double gearRatio) {
		super(port);
		mc = new MotorChecker();
		this.gearRatio=gearRatio;
	}

	/**Combination of setSpeed and rotateTo always minimises angle
	 * @param limitAngle - Absolute angle to turn to
	 * @param speed - speed in deg/sec*
	 * @param immediateReturn - returns before or after motion*/
	public void moveTo(double limitAngle, int speed, boolean immediateReturn){
		setGearSpeed(speed);
		rotateGearTo(limitAngle, immediateReturn, true);
	}
	
	/**adds support for gearRatio
	 * @param speed - speed of object at end of motor after gears*/
	public void setGearSpeed (double speed){	
		setSpeed( (int)(speed*gearRatio) );
	}
	/**adds support for gearRatio
	 * @return speed of object at end of motor after gears*/
	public double getGearSpeed(){
		return getSpeed()/gearRatio;
	}
	/**adds support for gearRatio(uses tacho counts)
	 * @return actual speed of object at end of motor after gears*/
	public double getGearRotationSpeed(){
		return getRotationSpeed()/gearRatio;
	}

	/**relative rotation motion calls rotateGearTo on angle+tacho
	 * @param angle - the angle to turn by
	 * @param immediateReturn - weather ot return immediately from the motion or not
	 * @return rotateGearTo(getGearTachoCount()+angle, immediateReturn, false);
	 * @see EnhancedMotor#rotateGearTo(double limitAngle, boolean immediateReturn, boolean minimiseRotation)*/
	
	public double rotateGear(double angle, boolean immediateReturn){
		return rotateGearTo(getGearTachoCount()+angle, immediateReturn, false);
	}
	
	
	/**rotates final gear to limitAngle adds option for minimising rotation
	 *  and checking to see if the motor is stuck. does not return until
	 *  motor has started moving or given up on trying
	 * @param limitAngle - angle to rotate final gear to
	 * @param immediateReturn - returns before or after motion
	 * @param minimiseRotation - weather or not to minimise turning angle
	 * @return if(immediateReturn){ delay till motion ends }else{ if(stuck){-1}else{0} }*/
	
	public double rotateGearTo(double limitAngle, boolean immediateReturn, boolean minimiseRotation){
		long wait=System.currentTimeMillis();
		while(isMoving() && System.currentTimeMillis()-wait<1000) Thread.yield();
		stop();
		//mc.cancelCheck();
		double cGTacho=getGearTachoCount();
		double delay;
		if(minimiseRotation){
			delay = 1100.0/getGearSpeed()*Math.abs(Trig.fixAngle2(cGTacho-limitAngle));
			limitAngle=minRot(limitAngle, cGTacho);	
		}else{
			delay = 1100.0/getGearSpeed()*Math.abs(limitAngle-cGTacho);
		}
		if(autoStop)mc.newCheckIn(delay);
		//if(!autoStop)rotateTo((int)(limitAngle*gearRatio), true);	
		/*else*/ nr.rotateTo((int)(limitAngle*gearRatio));//TODO rawr
		wait=System.currentTimeMillis();
		while(!isMoving() && System.currentTimeMillis()-wait<1000) Thread.yield();
		if(immediateReturn) return delay;
		while(isReallyMoving()) Thread.yield();
		if(mc.stuck)return -1;
		return 0;
		
	}
	/**does not work if speed<10 (speed!=gearSpeed)*/
	public boolean isReallyMoving(){
		return !(getRotationSpeed()==0);
	}
	
	/**@return gearRatio*tachoCount*/
	public double getGearTachoCount() {	
		return gearRatio*getTachoCount();
	}

	/**helper method for rotateTo, fixes limitAngle so that rotation is minimised
	 * @param limitAngle - angle(deg) to rotate to, unformatted
	 * @param cTacho - current tacho count
	 * @return formatted angle*/
	private double minRot(double limitAngle, double cTacho){
		limitAngle=(int) Trig.fixAngle(limitAngle);
		if((cTacho-limitAngle)>180)limitAngle+=360;
		else if((cTacho-limitAngle)<-180)limitAngle-=360;
	
		return limitAngle;
	}
	/**internal class for motion without regulation*/
	private class NoRegulator extends Thread{
		private int limitAngle;
		private EnhancedMotor m;
		
		/**Constructor autostarts the thread
		 * @param m - current motor*/
		public NoRegulator(EnhancedMotor m){
			this.m=m;
			start();
		}
		/**always immediate return*/
		public void rotateTo(int limitAngle){
			this.limitAngle=limitAngle;		
			this.interrupt();
		}
		public void run(){
			while(true){
				try{Thread.sleep(60000);}
				catch(Exception e){
					forward();
					m.stop();
					if(limitAngle > getTachoCount()) forward();
			        else backward();
					while(!isMoving())Thread.yield();
					while(Math.abs(limitAngle-getTachoCount())>2)Thread.yield();
					m.stop();
				}
			}
		}
	}
	/**only used to check if the cancel checked worked properly
	 * to be removed before final version imo*/
	public void testChecker(){
		MotorChecker moch = new MotorChecker();
		moch.newCheckIn(2000);
		try {
			Thread.sleep(1000);
		} catch (InterruptedException e) {}
		moch.cancelCheck();
		
	}
	
	/**Internal class used to check if motor is stuck
	 * the only reason this class exists is that Motor already has a timedOut()
	 * method in it and so its subclass could not extend TimerListener
	 * without overwriting that method */
	private class MotorChecker implements TimerListener{
		/**Timer for check*/
		Timer2 t;
		
		/**boolean to indicate the last motion got stuck
		 * gets reset on cancelCheck calls*/
		boolean stuck=false;
		
		/**Constructor creates timer*/
		public MotorChecker(){
			t=new Timer2(1337, this);			
		}
		/**sets the delay and starts the timer
		 * @param milis timer delay*/
		public void newCheckIn(double milis){
			cancelCheck();
			t.setDelay((int)milis);
			t.start();
			//System.out.println("called");
		}	
		/**cancels the last checj so it doesnt stop the current motion early*/
		public void cancelCheck(){
			//System.out.println("canceled");
			t.stop();
			stuck=false;
			
		}
		
		/**Stops the EnhancedMotor*/
		@Override
		public void timedOut() {
			t.stop();
			stop();
			stuck=true;

			
		}
	}
	
	//accessors
	/**accessor
	 * @return gearRatio*/
	public double getGearRatio(){
		return gearRatio;
	}
	
	/**accessor
	 * @return autoStop*/
	public boolean getAutoStop(){
		return autoStop;
	}
	
	
	//mutators	
	/**gearRatio mutator, only takes effect after end of current motion
	 * @param gearRatio - new value for gear ratio */
	public void setGearRatio(double gearRatio){
		this.gearRatio=gearRatio;
	}
	
	/**autoStop mutator, only takes effect after end of current motion
	 * @param autoStop -  new value for autoStop*/
	public void setAutoStop(boolean autoStop){
		this.autoStop=autoStop;
	}
	
}